﻿/************************************************************************************ 
 * PSMoveCalibration.cs
 * 
 * Author      : Hristo Hristov
 * Datum       : 28.04.2011
 * Version     : 1.0
 * Revision    : 5
 * 
 *************************************************************************************/


using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Runtime.InteropServices;

using Microsoft.Xna.Framework;
using Microsoft.Win32.SafeHandles;
using Microsoft.VisualBasic;
using Microsoft.Xna.Framework.Graphics;
using System.Threading;
using System.Diagnostics;
using NOVA.Scenery;
using NOVA.Physics;
using Emgu.CV;
using Emgu.CV.Structure;
using NOVA.UI;


namespace NOVA.Components.Move
{
    /// <summary>
    /// Playstation Move calibration states.
    /// </summary>
    public enum CalibrationState
    {
        Idle,
        SetColor,
        VerifyStatus,
        Calculation,
        VerifyResults,
        Calibrated,
        NotCalibratable
    };

    unsafe struct copystructVGA
    {
        // Size of a VGA image (640*480), where one pixel is represented 
        // 4-byte integer
        fixed int l[307200];
    }  

    public class tracker 
    {
            public int initialized = 0;
            public int seqnum = 0;
            public float[][] R = new float[3][];  // Rotates device coordinates to global coordinates.
                        // R[0] is global x-axis (EAST) expressed in local coordinates.
                        // R[1] is global y-axis (NORTH) expressed in local coordinates.
                        // R[2] is global z-axis (UP) expressed in local coordinates.
            public float[] p = new float[3];     // Position of device origin in global frame.
            public float[] v = new float[3];     // Velocity of device origin in global frame.
            public float[] A0 = new float[3];    // Lowpass-filtered acceleration in local coordinates.
            public float Gz0 = 0.0f;      // Lowpass-filtered Z gyro.
    };

    /// <summary>
    /// A Playstation Move controller calibration class, which the Playstation
    /// Eye camera and a state machine to calibrate the color and rotation of 
    /// the Move controller.
    /// </summary>
    public class PSMoveCalibration : DrawableGameComponent
    {
        #region Members
        public ImageProcessing imgProc = new ImageProcessing();
        public PSMoveController moveController = new PSMoveController();
        Hid moveHid = new Hid();
        DeviceManagement moveDeviceManagement = new DeviceManagement();
        GraphicsDevice graphicsDevice;

        String moveDevicePathName;
        String hidUsage;

        SafeFileHandle readHandle;
        SafeFileHandle writeHandle;
        SafeFileHandle hidHandle;

        IntPtr deviceNotificationHandle;
        IntPtr moveWindowHandle = IntPtr.Zero;
        
        CylinderObject box;

        Boolean exclusiveAccess;
        Boolean bDeviceDetected;
        bool m_bCalibrated;
        bool m_bNotCalibratable;

        SpriteBatch spriteBatch;
        Color m_color = Color.Black;
        int m_iColor = 0;
        int m_iThreshold = 250;
        Vector3 m_position;
        double m_distance;
        double m_iBlinkTimeout;
        bool m_bBlinkTimer;

        Timer timerDataProc;
        Thread threadDataProc;

        Vector3 m_gyrRef;
        Matrix m_matRef;
        Matrix m_mat;
        IirFilter[] m_pcLowPassFilterAcc = new IirFilter[3];
        IirFilter[] m_pcLowPassFilterMag = new IirFilter[3];
        IirFilter[] m_pcLowPassFilterGyr = new IirFilter[3];
        tracker dev = new tracker();
        List<Color> colors = new List<Color>(5);
        double[] radiusList = new double[5];
        int iCalibrationStep;
        public CalibrationState currentState = CalibrationState.Idle;

        Matrix oldRot = Matrix.Identity;

#if DEBUG
        int frameRateU = 0;
        int frameRateD = 0;
        int frameCounterU = 0;
        int frameCounterD = 0;
        TimeSpan elapsedTimeU = TimeSpan.Zero;
        TimeSpan elapsedTimeD = TimeSpan.Zero;
        
        Stopwatch w;
#endif
        #endregion

        public Vector2 ScreenPosition { get; set; }
        public Quaternion Orientation { get; set; }

        /// <summary>
        /// A Playstation Move controller calibration class, which can be used
        /// to calibrate the color and rotation data.
        /// </summary>
        public PSMoveCalibration(Game game)
            : base(game)
        {
            imgProc.setCameraParameters(1, 1, 1, 100, 100, 50, 50, 50);

            moveWindowHandle = game.Window.Handle;

            graphicsDevice = game.GraphicsDevice;
            spriteBatch = new SpriteBatch(graphicsDevice);

            m_matRef = Matrix.Identity;
            m_gyrRef = Vector3.Zero;
            // Contruct simple 1-pole low-pass filters for sensor data
            for (int i = 0; i < 3; i++)
            {
                m_pcLowPassFilterAcc[i] = new IirFilter();
                m_pcLowPassFilterMag[i] = new IirFilter();
                m_pcLowPassFilterGyr[i] = new IirFilter();
            }

            m_position = Vector3.Zero;
            Orientation = Quaternion.Identity;
            m_distance = 0.0;
            m_iBlinkTimeout = 0.0;

            m_bNotCalibratable = false;
            m_bBlinkTimer = false;

            //m_scene = new GoblinXNA.SceneGraph.Scene(this.Game);
            //m_scene.Enabled = false;
            //m_scene.PreferPerPixelLighting = true;
            
            Core.ShowFPS = true;
            Core.ShowNotifications = true;
            Core.ShowTriangleCount = true;
            
            //m_scene.PhysicsEngine = new BEPUPhysics();            

            colors.Add(Color.Red);
            colors.Add(new Color(0f, 255f, 0f));    // Green
            colors.Add(new Color(0f, 0f, 255f)); // Blue
            //colors.Add(new Color(255f, 0f, 255f));
            //colors.Add(Color.Cyan);
            iCalibrationStep = 0;

            // Draw the scene before the local drawing stuff
            //this.DrawOrder = m_scene.DrawOrder + 1;
            //game.Components.Add(this);

            dev.R = new float[][] { new float[] { 1, 0, 0 }, new float[] { 0, 1, 0 }, new float[] { 0, 0, 1 } };

            //CreateModel();

            // Create the thread for the data processing
            //threadDataProc = new Thread(callbackDataProc);
            //ThreadPool.QueueUserWorkItem(callbackDataProc);
        }

        /// <summary>
        /// Initializes the Move controller and the calibration 
        /// </summary>
        public override void Initialize()
        {
            try
            {
                StartupMove();
            }
            catch (Exception ex)
            {
                DisplayException("Move Initialition", ex);
                throw;
            }

            base.Initialize();
        }

        /// <summary>
        /// TO_DELETE
        /// </summary>
        /// <param name="gameTime"></param>
        public override void Draw(GameTime gameTime)
        {
#if DEBUG 
            elapsedTimeD += gameTime.ElapsedGameTime;

            frameCounterD++;

            if (elapsedTimeD > TimeSpan.FromSeconds(1))
            {
                elapsedTimeD -= TimeSpan.FromSeconds(1);
                frameRateD = frameCounterD;
                frameCounterD = 0;
            }
#endif
            
            //Texture2D tex = new Texture2D(graphicsDevice, 640, 480, false, SurfaceFormat.Color);
            
            //int[] array = new int[640*480];

            //int index = 0;

            //// Determine the image stride
            //int srcStride = 640 * 4;
            
            //// Copy the data from the pointer of the PS Eye interface into the integer array
            //// using only the RGB values
            ////
            //// Value:       R   |    G    |     B    |  unused
            //// Bits:    0......7|8......15|16......23|24......31
            //unsafe
            //{
            //    byte* src = (byte*)(imgProc.Eye.ptrBmpPixels);

            //    for (int i = 0; i < 480; i++)
            //    {
            //        for (int j = 0; j < srcStride; j += 4)
            //        {
            //            if (currentState == CalibrationState.NotCalibratable)
            //            {
            //                if (imgProc.result[new System.Drawing.Point(j >> 2, i)].Intensity > 0)
            //                    array[index++] = 255;
            //                else
            //                    array[index++] = (int)((*(src + j) << 16) | (*(src + j + 1) << 8) | *(src + j + 2));
            //            }
            //            else
            //                array[index++] = (int)((*(src + j) << 16) | (*(src + j + 1) << 8) | *(src + j + 2));
            //        }

            //        src += srcStride;
            //    }
            //}
            
            //// Create the texture from the camera image
            //tex.SetData<int>(array, 0, 640 * 480); 

            // Let the scene draw the texture
            //m_scene.BackgroundTexture = GetCameraImage();//tex;
            
#if DEBUG
            SpriteFont font = Game.Content.Load<SpriteFont>("DebugFont");
            string str = String.Format("FPS_U: {0}; FPS_D: {1}\n X: {2}, Y:{3}, Z: {4} \n R: {5}; G: {6}; B: {7}", frameRateU.ToString(), frameRateD.ToString(), m_position.X.ToString(), m_position.Y.ToString(), m_position.Z.ToString(), radiusList[0].ToString(), radiusList[1].ToString(), radiusList[2].ToString());
            spriteBatch.Begin();
            spriteBatch.DrawString(font, str, new Vector2(75.0f, Game.Window.ClientBounds.Height - 125.0f), Color.White);
            spriteBatch.End();
#endif
            
            // The scene is drawn here
            base.Draw(gameTime);          
        }

        /// <summary>
        /// TO_DELETE
        /// </summary>
        /// <param name="gameTime"></param>
        public override void Update(GameTime gameTime)
        {
#if DEBUG
            elapsedTimeU += gameTime.ElapsedGameTime;

            frameCounterU++; 

            if (elapsedTimeU > TimeSpan.FromSeconds(1))
            {
                elapsedTimeU -= TimeSpan.FromSeconds(1);
                frameRateU = frameCounterU;
                frameCounterU = 0;
            }
#endif

            switch (currentState)
            {
                case CalibrationState.Idle:

                    ReadFeatureReports();

                    if (moveController.psButtons.MOVE_MOVE_BUTTON)
                        currentState = CalibrationState.SetColor;

                    break;
                case CalibrationState.SetColor:
                 
                    while (iCalibrationStep != 3)
                    {
                        // Set the next color
                        moveController.Color = colors[iCalibrationStep];
                        imgProc.TrackingColor = colors[iCalibrationStep];

                        WriteFeatureReports();

                        Thread.Sleep(100);
                        imgProc.Eye.autoResetEvent.WaitOne(1000);

                        radiusList[iCalibrationStep] = imgProc.GetBulbRadius();

                        iCalibrationStep++;
                    }

                    currentState = CalibrationState.VerifyResults;

                    break;
                //case CalibrationState.VerifyStatus:

                //    ReadFeatureReports();

                //    if (moveController.Color == colors[iCalibrationStep])
                //        currentState = CalibrationState.Calculation;

                //    break;
                //case CalibrationState.Calculation:

                //    radiusList[iCalibrationStep] = imgProc.GetBulbRadius();

                //    if (iCalibrationStep < 5)
                //        iCalibrationStep++;

                //    if (iCalibrationStep == 5)
                //        currentState = CalibrationState.VerifyResults;
                //    else
                //        currentState = CalibrationState.SetColor;

                //    break;
                case CalibrationState.VerifyResults:

                    if (radiusList[0] > 0 || radiusList[1] > 0 || radiusList[2] > 0 )//|| radiusList[3] > 0 || radiusList[4] > 0)
                    {
                        // Find the largest radius
                        int index = 0;
                        double tmp = radiusList[0];
                        for (int i = 1; i < 3; i++)
                        {
                            if (radiusList[i] > tmp)
                            {
                                tmp = radiusList[i];
                                index = i;
                            }
                        }

                        // Set the final color
                        moveController.Color = colors[index];
                        imgProc.TrackingColor = colors[index];

                        ExchangeFeatureReports();

                        EngineSettings.MoveControllerColor = colors[index];
                            
                        //CreateModel();   
                 
                        // Calculate the initial orientation
                        UpdateMoveTranslation();
                        //UpdateMoveRotation(true);                        

                        imgProc.calibrated = true;
                        m_bCalibrated = true;
                        imgProc.threading = true;

                        currentState = CalibrationState.Calibrated;                        
                    }
                    else
                    {
                        iCalibrationStep = 0;

                        for (int i = 0; i < 3; i++)
                            radiusList[i] = 0;

                        currentState = CalibrationState.NotCalibratable;
                    }

                    break;
                case CalibrationState.Calibrated:

                    moveController.Color = EngineSettings.MoveControllerColor;
                    ExchangeFeatureReports();

                    if (moveController.psButtons.MOVE_MOVE_BUTTON)
                    {
                        iCalibrationStep = 0;

                        for (int i = 0; i < 3; i++)
                            radiusList[i] = 0;

                        //m_scene.RootNode.RemoveChildren();

                        imgProc.calibrated = false;
                        m_bCalibrated = false;
                        imgProc.threading = false;

                        EngineSettings.MoveControllerColor = Color.Black;

                        moveController.Color  = Color.Black;
                        imgProc.TrackingColor = Color.Black;

                        currentState = CalibrationState.SetColor; 
                    }

                    ScreenPosition = imgProc.Position;

                    Orientation = CalcRotation();
                    //else
                    //{
                    //    UpdateMoveTranslation();
                    //    //UpdateMoveRotation(false);
                    //}

                    break;
                case CalibrationState.NotCalibratable:
                    
                    // The controller can not be found in marked area
                    // and therefore not calibratable.

                    // Let the controller blink red all 500ms
                    m_iBlinkTimeout += gameTime.ElapsedGameTime.Milliseconds;

                    if (m_iBlinkTimeout >= 200)
                    {
                        m_bBlinkTimer = !m_bBlinkTimer;
                        m_iBlinkTimeout = 0.0;
                    }

                    if (m_bBlinkTimer)
                        moveController.Color = new Color(255, 0, 0);
                    else
                        moveController.Color = new Color(0, 0, 0);

                    // Transmit the data
                    ExchangeFeatureReports();

                    if (moveController.psButtons.MOVE_MOVE_BUTTON)
                        currentState = CalibrationState.SetColor;

                    break;
            }

#if OLD
            // Set the color found at the calibration state. On startup this
            // color is "zero", because there was no calibration yet.
            moveController.setColor(EngineSettings.MoveControllerColor.R,
                                    EngineSettings.MoveControllerColor.G,
                                    EngineSettings.MoveControllerColor.B);

            // Set the color and read the controller button state
            ExchangeFeatureReports();

            // Start calibration, when the Move-button is pressed
            if(moveController.psButtons.MOVE_MOVE_BUTTON && !m_bCalibrated)
            {
                //// New calibration is starting
                //m_scene.RootNode.RemoveChildren();
                //m_bNotCalibratable = false;

                // Take a snapshot the background and determine the radius
                //if(iCalibrationStep != 0)
                //    radiusList[iCalibrationStep-1] = imgProc.GetBulbRadius();

                // Set the next color
                moveController.setColor(colors[iCalibrationStep].R, colors[iCalibrationStep].G, colors[iCalibrationStep].B);
                imgProc.TrackingColor = colors[iCalibrationStep];

                // Transmit the data
                ExchangeFeatureReports();

                //Thread.Sleep(100);        
                
                radiusList[iCalibrationStep] = imgProc.GetBulbRadius();

                if (iCalibrationStep < 5)
                    iCalibrationStep++;

                if (iCalibrationStep == 5)
                {
                    // Check if all values are valid
                    if (radiusList[0] > 0 && radiusList[1] > 0 && radiusList[2] > 0 && radiusList[3] > 0 && radiusList[4] > 0)
                    {
                        // Find the largest radius
                        int index = 0;
                        int tmp = radiusList[0];
                        for (int i = 1; i < radiusList.Count; i++)
                        {
                            if (tmp > radiusList[0])
                            {
                                tmp = radiusList[i];
                                index = i;
                            }
                        }

                        // Save the best color
                        EngineSettings.MoveControllerColor = colors[index];
                        imgProc.TrackingColor = colors[index];

                        // Assign the best color to the controller
                        moveController.setColor(colors[index].R, colors[index].G, colors[index].B);

                        CreateModel();

                        m_bCalibrated = true;
                    }
                    else
                    {
                        // No bulb was found (maybe not visible area of the camera) => Not calibratable
                        m_bNotCalibratable = true;
                    }
                }
            }
            else if (m_bNotCalibratable)
            {
                #region Not calibratable
                // The controller can not be found in marked area
                // and therefore not calibratable.

                // Let the controller blink red all 500ms
                m_iBlinkTimeout += gameTime.ElapsedGameTime.Milliseconds;

                if (m_iBlinkTimeout >= 200)
                {
                    m_bBlinkTimer = !m_bBlinkTimer;
                    m_iBlinkTimeout = 0.0;
                }

                if (m_bBlinkTimer)
                    moveController.setColor(255, 0, 0);
                else
                    moveController.setColor(0, 0, 0);

                // Transmit the data
                ExchangeFeatureReports();
                #endregion
            }
            else if (m_bCalibrated)
            {
                UpdateMoveTranslation();
                UpdateMoveRotation(false);
            }            
#endif
            base.Update(gameTime);
        }
               
        /// <summary>
        /// Calculate the current rotation matrix of the controller
        /// </summary>
        /// <returns></returns>
        private Quaternion CalcRotation()
        {
            Quaternion rotation = Quaternion.Identity;
            
            int seqnum = 0;
            float[] a = new float[3];
            float[] g = new float[3];
            float[] m = new float[3];

            int iAccMinX = EngineSettings.AccMinX;
            int iAccMaxX = EngineSettings.AccMaxX;
            int iAccMinY = EngineSettings.AccMinY;
            int iAccMaxY = EngineSettings.AccMaxY;
            int iAccMinZ = EngineSettings.AccMinZ;
            int iAccMaxZ = EngineSettings.AccMaxZ;
            int iMagMinX = EngineSettings.MagMinX;
            int iMagMaxX = EngineSettings.MagMaxX;
            int iMagMinY = EngineSettings.MagMinY;
            int iMagMaxY = EngineSettings.MagMaxY;
            int iMagMinZ = EngineSettings.MagMinZ;
            int iMagMaxZ = EngineSettings.MagMaxZ;
            int iGyrMinX = EngineSettings.GyrMinX;
            int iGyrMaxX = EngineSettings.GyrMaxX;
            int iGyrMinY = EngineSettings.GyrMinY;
            int iGyrMaxY = EngineSettings.GyrMaxY;
            int iGyrMinZ = EngineSettings.GyrMinZ;
            int iGyrMaxZ = EngineSettings.GyrMaxZ;
            
            a[0] = (float)(2 * moveController.inputReport.RawForceX - (iAccMaxX + iAccMinX)) / (iAccMaxX - iAccMinX);
            a[1] = (float)(2 * moveController.inputReport.RawForceY - (iAccMaxY + iAccMinY)) / (iAccMaxY - iAccMinY);
            a[2] = (float)(2 * moveController.inputReport.RawForceZ - (iAccMaxZ + iAccMinZ)) / (iAccMaxZ - iAccMinZ);
            m[0] = (float)(2 * moveController.inputReport.RawMagnetX - (iMagMaxX + iMagMinX)) / (iMagMaxX - iMagMinX);
            m[1] = (float)(2 * moveController.inputReport.RawMagnetY - (iMagMaxY + iMagMinY)) / (iMagMaxY - iMagMinY);
            m[2] = (float)(2 * moveController.inputReport.RawMagnetZ - (iMagMaxZ + iMagMinZ)) / (iMagMaxZ - iMagMinZ);
            g[0] = (float)(2 * moveController.inputReport.RawGyroYaw - (iGyrMaxX + iGyrMinX)) / (iGyrMaxX - iGyrMinX);
            g[1] = (float)(2 * moveController.inputReport.RawGyroPitch - (iGyrMaxY + iGyrMinY)) / (iGyrMaxY - iGyrMinY);
            g[2] = (float)(2 * moveController.inputReport.RawGyroRoll - (iGyrMaxZ + iGyrMinZ)) / (iGyrMaxZ - iGyrMinZ);
            

            string str = string.Format("{0}, {1}, {2}", a[0], a[1], a[2]);
            UI2DRenderer.WriteText(Vector2.Zero, str, Color.Red, Core.EmbeddedContent.Load<SpriteFont>("DebugFont"), UI2DRenderer.HorizontalAlignment.Center, UI2DRenderer.VerticalAlignment.Bottom);

            //float dt = 1.0f / 175.0f;

            //if (dev.initialized == 0)
            //    dev.seqnum = seqnum;
            //else
            //    dt *= (seqnum - dev.seqnum) & 31;

            //dev.seqnum = seqnum;
            //update_tracker_AGM(ref dev, dt, a, g, m);

            Matrix mat = Matrix.Identity;
            float[][] R = new float[][] { new float[] { 1, 0, 0 }, new float[] { 0, 1, 0 }, new float[] { 0, 0, 1 } };

            float[,] OmegaX = { {     0, -g[2],  g[1] },
		                        {  g[2],     0, -g[0] },
		                        { -g[1],  g[0],     0 } };

            float[][] oldR = new float[][] { new float[] { 1, 0, 0 }, new float[] { 0, 1, 0 }, new float[] { 0, 0, 1 } };
            float[][] R3 = new float[][] { new float[] { 0, 0, 0 }, new float[] { 0, 0, 0 }, new float[] { 0, 0, 0 } };

            oldR[0][0] = oldRot.M11; oldR[0][1] = oldRot.M12; oldR[0][2] = oldRot.M13;
            oldR[1][0] = oldRot.M21; oldR[1][1] = oldRot.M22; oldR[1][2] = oldRot.M23;
            oldR[2][0] = oldRot.M31; oldR[2][1] = oldRot.M32; oldR[2][2] = oldRot.M33;

            //// Integrate rotation rate: R += R * Omega_X * dt
            //for (int i = 0; i < 3; ++i)
            //    for (int j = 0; j < 3; ++j)
            //        for (int k = 0; k < 3; ++k)
            //            R3[i][j] += oldR[i][k] * OmegaX[k, j] * (1 / 60f);

            // get Z axis from normalised accelerometer data (gravity)
            R[0][0] = a[0];
            R[0][1] = a[1];
            R[0][2] = a[2];
            normalize(R[2], out R[2]);

            // calculate EAST axis from Z axis and normalised magnetometer data
            R[2][0] = -m[0];
            R[2][1] = -m[1];
            R[2][2] = -m[2];
            cross(R[2], R[0], out R[0]);
            normalize(R[0], out R[0]);

            // calculate NORTH axis from Z axis and EAST axis
            cross(R[2], R[0], out R[1]);

            float kR = 0.18f;
            float[][] R0 = R;
            for (int i = 0; i < 3; ++i)
                for (int j = 0; j < 3; ++j)
                    R3[i][j] = R[i][j] * kR + R3[i][j] * (1 - kR);

            normalize(R3[2], out R3[2]);
            cross(R3[2], R3[0], out R3[1]);
            normalize(R3[1], out R3[1]);
            cross(R3[1], R3[2], out R3[0]);

            
            mat.M11 = R3[0][0]; mat.M12 = R3[0][1]; mat.M13 = R3[0][2];
            mat.M21 = R3[1][0]; mat.M22 = R3[1][1]; mat.M23 = R3[1][2];
            mat.M31 = R3[2][0]; mat.M32 = R3[2][1]; mat.M33 = R3[2][2];


            float dt = 1.0f / 60.0f;

            //if (dev.initialized == 0)
            //    dev.seqnum = seqnum;
            //else
            //    dt *= (seqnum - dev.seqnum) & 31;

            dev.seqnum = seqnum;
            update_tracker_AGM(ref dev, dt, a, g, m);

            mat.M11 = dev.R[0][0]; mat.M12 = dev.R[0][1]; mat.M13 = dev.R[0][2];
            mat.M21 = dev.R[1][0]; mat.M22 = dev.R[1][1]; mat.M23 = dev.R[1][2];
            mat.M31 = dev.R[2][0]; mat.M32 = dev.R[2][1]; mat.M33 = dev.R[2][2];

            Matrix mat2 = Matrix.Multiply(EngineSettings.MoveControllerCalibRotation, mat);

            rotation = Quaternion.CreateFromRotationMatrix(mat2);

            if (EngineSettings.MoveControllerCalibRotation == Matrix.Identity)
            {
                EngineSettings.MoveControllerCalibRotation = mat;
            }

            oldRot = mat2;

            return rotation;
        }

        /// <summary>
        /// Start the move controller access.
        /// </summary>
        private void StartupMove()
        {
            try
            {
                FindTheHid();
            }
            catch (Exception ex)
            {
                DisplayException("Move Startup", ex);
                throw;
            }
        }

        /// <summary>
        /// Find the HID representing the move controller.
        /// </summary>
        private Boolean FindTheHid()
        {
            Boolean deviceFound = false;
            String[] devicePathName = new String[128];
            String functionName = "";
            Guid hidGuid = Guid.Empty;
            Int32 memberIndex = 0;
            Int16 moveProductID = 0;
            Int16 moveVendorID = 0;
            Boolean success = false;

            try
            {
                bDeviceDetected = false;

                // Set the Sony-specific vendor- and product-ID
                moveVendorID = Convert.ToInt16(Conversion.Val("&h" + "8888"));
                moveProductID = Convert.ToInt16(Conversion.Val("&h" + "0308"));

                //  ***
                //  API function: 'HidD_GetHidGuid

                //  Purpose: Retrieves the interface class GUID for the HID class.

                //  Accepts: 'A System.Guid object for storing the GUID.
                //  ***

                Hid.HidD_GetHidGuid(ref hidGuid);

                functionName = "GetHidGuid";

                //  Fill an array with the device path names of all attached HIDs
                deviceFound = moveDeviceManagement.FindDeviceFromGuid(hidGuid, ref devicePathName);

                //  If there is at least one HID, attempt to read the Vendor ID and Product ID
                //  of each device until there is a match or all devices have been examined
                if (deviceFound)
                {
                    memberIndex = 0;

                    do
                    {
                        //  ***
                        //  API function:
                        //  CreateFile

                        //  Purpose:
                        //  Retrieves a handle to a device.

                        //  Accepts:
                        //  A device path name returned by SetupDiGetDeviceInterfaceDetail
                        //  The type of access requested (read/write).
                        //  FILE_SHARE attributes to allow other processes to access the device while this handle is open.
                        //  A Security structure or IntPtr.Zero. 
                        //  A creation disposition value. Use OPEN_EXISTING for devices.
                        //  Flags and attributes for files. Not used for devices.
                        //  Handle to a template file. Not used.

                        //  Returns: a handle without read or write access.
                        //  This enables obtaining information about all HIDs, even system
                        //  keyboards and mice. 
                        //  Separate handles are used for reading and writing.
                        //  ***

                        hidHandle = FileIO.CreateFile(devicePathName[memberIndex], 0, FileIO.FILE_SHARE_READ | FileIO.FILE_SHARE_WRITE, IntPtr.Zero, FileIO.OPEN_EXISTING, 0, 0);

                        functionName = "CreateFile";

                        if (!hidHandle.IsInvalid)
                        {
                            //  The returned handle is valid, 
                            //  so find out if this is the device we're looking for.

                            //  Set the Size property of DeviceAttributes to the number of bytes in the structure.

                            moveHid.DeviceAttributes.Size = Marshal.SizeOf(moveHid.DeviceAttributes);

                            //  ***
                            //  API function:
                            //  HidD_GetAttributes

                            //  Purpose:
                            //  Retrieves a HIDD_ATTRIBUTES structure containing the Vendor ID, 
                            //  Product ID, and Product Version Number for a device.

                            //  Accepts:
                            //  A handle returned by CreateFile.
                            //  A pointer to receive a HIDD_ATTRIBUTES structure.

                            //  Returns:
                            //  True on success, False on failure.
                            //  ***                            
                            success = Hid.HidD_GetAttributes(hidHandle, ref moveHid.DeviceAttributes);

                            if (success)
                            {
                                //  Find out if the device matches the one we're looking for.
                                if ((moveHid.DeviceAttributes.VendorID == moveVendorID) && (moveHid.DeviceAttributes.ProductID == moveProductID))
                                {
                                    //  Display the information in form's list box
                                    bDeviceDetected = true;

                                    //  Save the DevicePathName for OnDeviceChange()
                                    moveDevicePathName = devicePathName[memberIndex];
                                }
                                else
                                {
                                    //  It's not a match, so close the handle
                                    bDeviceDetected = false;

                                    hidHandle.Close();
                                }
                            }
                            else
                            {
                                //  There was a problem in retrieving the information
                                bDeviceDetected = false;
                                hidHandle.Close();
                            }
                        }

                        //  Keep looking until we find the device or there are no devices left to examine
                        memberIndex = memberIndex + 1;
                    }
                    while (!((bDeviceDetected || (memberIndex == devicePathName.Length))));
                }

                if (bDeviceDetected)
                {
                    //  The device was detected!

                    // Register to receive notifications if the device is removed or attached. 
                    // The window handle is set at the game!! HRH (13.04.11)
                    success = moveDeviceManagement.RegisterForDeviceNotifications(moveDevicePathName, moveWindowHandle, hidGuid, ref deviceNotificationHandle);

                    //  Learn the capabilities of the device.
                    moveHid.Capabilities = moveHid.GetDeviceCapabilities(hidHandle);

                    ///////////////////////////////////////////////////////////////////
                    // The following is not essential! HRH (16.04.11)
                    if (success)
                    {
                        //  Find out if the device is a system mouse or keyboard.
                        hidUsage = moveHid.GetHidUsage(moveHid.Capabilities);

                        //  Get the Input report buffer size.
                        //  Get handles to use in requesting Input and Output reports.
                        readHandle = FileIO.CreateFile(moveDevicePathName, FileIO.GENERIC_READ, FileIO.FILE_SHARE_READ | FileIO.FILE_SHARE_WRITE, IntPtr.Zero, FileIO.OPEN_EXISTING, FileIO.FILE_FLAG_OVERLAPPED, 0);

                        functionName = "CreateFile, ReadHandle";

                        if (readHandle.IsInvalid)
                        {
                            exclusiveAccess = true;
                        }
                        else
                        {
                            writeHandle = FileIO.CreateFile(moveDevicePathName, FileIO.GENERIC_WRITE, FileIO.FILE_SHARE_READ | FileIO.FILE_SHARE_WRITE, IntPtr.Zero, FileIO.OPEN_EXISTING, 0, 0);

                            functionName = "CreateFile, WriteHandle";

                            //  Flush any waiting reports in the input buffer. (optional)
                            moveHid.FlushQueue(readHandle);
                        }
                    }
                }
                else
                {
                    //  The device wasn't detected.
                }

                return bDeviceDetected;
            }
            catch (Exception ex)
            {
                DisplayException("Find Move HID", ex);
                throw;
            }
        }

        /// <summary>
        /// TO_DELETE
        /// </summary>
        internal static void DisplayException(String moduleName, Exception e)
        {
            String message = null;

            //  Create an error message
            message = "Exception: " + e.Message + "Module: " + moduleName + "Method: " + e.TargetSite.Name;
            //Debug.Write(message);
        }

        /// <summary>
        /// Exchange the controller reports.
        /// </summary>
        private void ExchangeFeatureReports()
        {
            String byteValue = null;
            Int32 count = 0;
            Byte[] inFeatureReportBuffer = null;
            Byte[] outFeatureReportBuffer = null;
            Boolean success = false;

            try
            {
                Hid.InFeatureReport myInFeatureReport = new Hid.InFeatureReport();
                Hid.OutFeatureReport myOutFeatureReport = new Hid.OutFeatureReport();

                inFeatureReportBuffer = null;

                if ((moveHid.Capabilities.FeatureReportByteLength > 0))
                {
                    //  The HID has a Feature report.
                    outFeatureReportBuffer = new Byte[moveHid.Capabilities.FeatureReportByteLength];
                    outFeatureReportBuffer = moveController.setOutputReport();

                    //  Write a report to the device
                    success = myOutFeatureReport.Write(outFeatureReportBuffer, hidHandle);

                    //  Read a report from the device.

                    //  Set the size of the Feature report buffer. 
                    //  Subtract 1 from the value in the Capabilities structure because 
                    //  the array begins at index 0.

                    if ((moveHid.Capabilities.FeatureReportByteLength > 0))
                        inFeatureReportBuffer = new Byte[moveHid.Capabilities.FeatureReportByteLength];

                    //  Read a report.
                    myInFeatureReport.Read(hidHandle, readHandle, writeHandle, ref bDeviceDetected, ref inFeatureReportBuffer, ref success);

                    // Fill input variables
                    if (success)
                        moveController.setInputReports(inFeatureReportBuffer);
                }
            }
            catch (Exception ex)
            {
                DisplayException("Move ExchangeFeatureReports", ex);
                throw;
            }
        }

        /// <summary>
        /// Transmit the new data to the controller.
        /// </summary>
        private void WriteFeatureReports()
        {
            String byteValue = null;
            Int32 count = 0;
            Byte[] outFeatureReportBuffer = null;
            Boolean success = false;

            try
            {
                Hid.OutFeatureReport myOutFeatureReport = new Hid.OutFeatureReport();
                
                if ((moveHid.Capabilities.FeatureReportByteLength > 0))
                {
                    //  The HID has a Feature report.
                    outFeatureReportBuffer = new Byte[moveHid.Capabilities.FeatureReportByteLength];
                    outFeatureReportBuffer = moveController.setOutputReport();

                    //  Write a report to the device
                    success = myOutFeatureReport.Write(outFeatureReportBuffer, hidHandle);                   
                }
            }
            catch (Exception ex)
            {
                DisplayException("Move ExchangeFeatureReports", ex);
                throw;
            }
        }

        /// <summary>
        /// Read the current controller data.
        /// </summary>
        private void ReadFeatureReports()
        {
            String byteValue = null;
            Int32 count = 0;
            Byte[] inFeatureReportBuffer = null;
            Boolean success = false;

            try
            {
                Hid.InFeatureReport myInFeatureReport = new Hid.InFeatureReport();
                
                inFeatureReportBuffer = null;

                if ((moveHid.Capabilities.FeatureReportByteLength > 0))
                {
                    //  Read a report from the device.

                    //  Set the size of the Feature report buffer. 
                    //  Subtract 1 from the value in the Capabilities structure because 
                    //  the array begins at index 0.

                    if ((moveHid.Capabilities.FeatureReportByteLength > 0))
                        inFeatureReportBuffer = new Byte[moveHid.Capabilities.FeatureReportByteLength];

                    //  Read a report.
                    myInFeatureReport.Read(hidHandle, readHandle, writeHandle, ref bDeviceDetected, ref inFeatureReportBuffer, ref success);

                    // Fill input variables
                    if (success)
                        moveController.setInputReports(inFeatureReportBuffer);
                }
            }
            catch (Exception ex)
            {
                DisplayException("Move ExchangeFeatureReports", ex);
                throw;
            }
        }
                               
        private void UpdateMoveTranslation()
        {      
            // Track the controller for the color found during the calibration and find
            // the relative coordinates in screen space (independent of the screen resolution)
            // and the relativ distance depending on real distance and camera parameters.
            // temp.X ^= Relative X coordinate in screen space between 0 and 1
            // temp.Y ^= Relative Y coordinate in screen space between 0 and 1
            // temp.Z ^= Relative distance 
            //Vector3 temp = imgProc.GetBulbPosition();//XNAREngine.Settings.MoveControllerColor);            

            // Convert the relative screen coordinates to absolute depending on the current
            // screen resolution
            //int iX = (int)(x * Game.Window.ClientBounds.Width);
            //int iY = (int)(y * Game.Window.ClientBounds.Height);
            int iX = (int)(imgProc.Position.X * Game.Window.ClientBounds.Width);
            int iY = (int)(imgProc.Position.Y * Game.Window.ClientBounds.Height);

            if (iX > 0 && iY > 0 && iX < Game.Window.ClientBounds.Width && iY < Game.Window.ClientBounds.Height)
            {
                // Pay attention to the current camera view
                Matrix viewMatrix = Core.ViewMatrix;

                // Convert the position vector from screen to world space -> 2D to 3D conversion
                Vector3 trans = Game.GraphicsDevice.Viewport.Unproject(new Vector3(iX, iY, 0),
                                                            Core.ProjectionMatrix,
                                                            viewMatrix,
                                                            Matrix.Identity);

                // Use the distance to scale the vector
                m_position = Vector3.Multiply(trans, imgProc.Distance/*temp.Z*/);

                // Assign the new translation to the model
                box.Position = m_position;//m_trans.Translation = m_position;
            }
        }

        private void UpdateMoveRotation(bool bCalibrate)
        {
            // Use the accelerometer data of the controller to calculate the rotation
#if OLD
            Matrix mat = Matrix.Identity;
            Vector3 a, m, g, n;
            int iAccMinX = XNAREngine.Settings.AccMinX;
            int iAccMaxX = XNAREngine.Settings.AccMaxX;
            int iAccMinY = XNAREngine.Settings.AccMinY;
            int iAccMaxY = XNAREngine.Settings.AccMaxY;
            int iAccMinZ = XNAREngine.Settings.AccMinZ;
            int iAccMaxZ = XNAREngine.Settings.AccMaxZ;
            int iMagMinX = XNAREngine.Settings.MagMinX;
            int iMagMaxX = XNAREngine.Settings.MagMaxX;
            int iMagMinY = XNAREngine.Settings.MagMinY;
            int iMagMaxY = XNAREngine.Settings.MagMaxY;
            int iMagMinZ = XNAREngine.Settings.MagMinZ;
            int iMagMaxZ = XNAREngine.Settings.MagMaxZ;
            int iGyrMinX = XNAREngine.Settings.GyrMinX;
            int iGyrMaxX = XNAREngine.Settings.GyrMaxX;
            int iGyrMinY = XNAREngine.Settings.GyrMinY;
            int iGyrMaxY = XNAREngine.Settings.GyrMaxY;
            int iGyrMinZ = XNAREngine.Settings.GyrMinZ;
            int iGyrMaxZ = XNAREngine.Settings.GyrMaxZ;
            
            // Normalize raw data between [-1, 1]
            a.X = (float)(2 * moveController.inputReport.RawForceX - (iAccMaxX + iAccMinX)) / (iAccMaxX - iAccMinX);
            a.Y = (float)(2 * moveController.inputReport.RawForceY - (iAccMaxY + iAccMinY)) / (iAccMaxY - iAccMinY);
            a.Z = (float)(2 * moveController.inputReport.RawForceZ - (iAccMaxZ + iAccMinZ)) / (iAccMaxZ - iAccMinZ);
            m.X = (float)(2 * moveController.inputReport.RawMagnetX - (iMagMaxX + iMagMinX)) / (iMagMaxX - iMagMinX);
            m.Y = (float)(2 * moveController.inputReport.RawMagnetY - (iMagMaxY + iMagMinY)) / (iMagMaxY - iMagMinY);
            m.Z = (float)(2 * moveController.inputReport.RawMagnetZ - (iMagMaxZ + iMagMinZ)) / (iMagMaxZ - iMagMinZ);
            g.X = (float)(2 * moveController.inputReport.RawGyroYaw - (iGyrMaxX + iGyrMinX)) / (iGyrMaxX - iGyrMinX);
            g.Y = (float)(2 * moveController.inputReport.RawGyroPitch - (iGyrMaxY + iGyrMinY)) / (iGyrMaxY - iGyrMinY);
            g.Z = (float)(2 * moveController.inputReport.RawGyroRoll - (iGyrMaxZ + iGyrMinZ)) / (iGyrMaxZ - iGyrMinZ);


            // Apply low-pass filter to sensor data
            a.X = m_pcLowPassFilterAcc[0].Step(a.X);
            a.Y = m_pcLowPassFilterAcc[1].Step(a.Y);
            a.Z = m_pcLowPassFilterAcc[2].Step(a.Z);
            m.X = m_pcLowPassFilterMag[0].Step(m.X);
            m.Y = m_pcLowPassFilterMag[1].Step(m.Y);
            m.Z = m_pcLowPassFilterMag[2].Step(m.Z);
            g.X = m_pcLowPassFilterGyr[0].Step(g.X);
            g.Y = m_pcLowPassFilterGyr[1].Step(g.Y);
            g.Z = m_pcLowPassFilterGyr[2].Step(g.Z);

            // Get Z axis from normalised accelerometer data (gravity)
            m.Normalize();
            mat.M13 = m.X;
            mat.M23 = m.Y;
            mat.M33 = m.Z;
            

            // Calculate EAST axis from Z axis and normalised magnetometer data            
            a.Normalize();
            mat.M12 = -a.X;
            mat.M12 = -a.Y;
            mat.M32 = -a.Z;
            a = Vector3.Cross(a, m);
            a.Normalize();
            

            // Calculate NORTH axis from Z axis and EAST axis
            n = Vector3.Cross(a, m);
            n.Normalize();
            mat.M11 = -n.X;
            mat.M21 = -n.Y;
            mat.M21 = -n.Z;
                        
            
            // Set reference matrix only during calibration
            if(m_matRef == Matrix.Identity)
                m_matRef = mat;            
            else
                mat = Matrix.Multiply(m_matRef, mat);

            Matrix c = Matrix.Identity;
            c.M11 = -1;
            c.M22 = -1;

            //mat = Matrix.Multiply(c, mat);

            if (m_gyrRef != Vector3.Zero)
            {
                //if (g.X < m_gyrRef.X + 10.0f && g.X > m_gyrRef.X - 10.0f &&
                //    g.Y < m_gyrRef.Y + 10.0f && g.Y > m_gyrRef.Y - 10.0f &&
                //    g.Z < m_gyrRef.Z + 10.0f && g.Z > m_gyrRef.Z - 10.0f)
                //{
                    g = Vector3.Subtract(g, m_gyrRef);
                    m_gyrRef = g;
                //}
            }
            else
                m_gyrRef = g;

            Matrix matA = Matrix.CreateFromYawPitchRoll(a.Y, a.Z, a.X);//g.X, g.Y,g.Z);//0, -a.Z, -a.X);//a.X=m.X=roll,a.Z=pitch,
            Matrix matG = Matrix.CreateFromYawPitchRoll(MathHelper.ToRadians(g.X * 2 * MathHelper.Pi), MathHelper.ToRadians(g.Y * 2 * MathHelper.Pi), MathHelper.ToRadians(g.Z * 2 * MathHelper.Pi));//0, -a.Z, -a.X);//a.X=m.X=roll,a.Z=pitch,
            Matrix matM = Matrix.CreateFromYawPitchRoll(m.Y, m.X, m.Z);//g.X, g.Y,g.Z);//0, -a.Z, -a.X);//a.X=m.X=roll,a.Z=pitch,

            //mat = Matrix.Multiply(m_matRef, mat);
            //mat = Matrix.Multiply(mat, matM);
            //mat = Matrix.Multiply(mat, matA);
            //mat = Matrix.Multiply(mat, matG);  
                        
            m_trans.Rotation = Quaternion.CreateFromRotationMatrix(mat);
#endif
            int seqnum = 0;
            float[] a = new float[3];
            float[] g = new float[3];
            float[] m = new float[3];

            int iAccMinX = EngineSettings.AccMinX;
            int iAccMaxX = EngineSettings.AccMaxX;
            int iAccMinY = EngineSettings.AccMinY;
            int iAccMaxY = EngineSettings.AccMaxY;
            int iAccMinZ = EngineSettings.AccMinZ;
            int iAccMaxZ = EngineSettings.AccMaxZ;
            int iMagMinX = EngineSettings.MagMinX;
            int iMagMaxX = EngineSettings.MagMaxX;
            int iMagMinY = EngineSettings.MagMinY;
            int iMagMaxY = EngineSettings.MagMaxY;
            int iMagMinZ = EngineSettings.MagMinZ;
            int iMagMaxZ = EngineSettings.MagMaxZ;
            int iGyrMinX = EngineSettings.GyrMinX;
            int iGyrMaxX = EngineSettings.GyrMaxX;
            int iGyrMinY = EngineSettings.GyrMinY;
            int iGyrMaxY = EngineSettings.GyrMaxY;
            int iGyrMinZ = EngineSettings.GyrMinZ;
            int iGyrMaxZ = EngineSettings.GyrMaxZ;

            a[0] = (float)(2 * moveController.inputReport.RawForceX - (iAccMaxX + iAccMinX)) / (iAccMaxX - iAccMinX);
            a[1] = (float)(2 * moveController.inputReport.RawForceY - (iAccMaxY + iAccMinY)) / (iAccMaxY - iAccMinY);
            a[2] = (float)(2 * moveController.inputReport.RawForceZ - (iAccMaxZ + iAccMinZ)) / (iAccMaxZ - iAccMinZ);
            m[0] = (float)(2 * moveController.inputReport.RawMagnetX - (iMagMaxX + iMagMinX)) / (iMagMaxX - iMagMinX);
            m[1] = (float)(2 * moveController.inputReport.RawMagnetY - (iMagMaxY + iMagMinY)) / (iMagMaxY - iMagMinY);
            m[2] = (float)(2 * moveController.inputReport.RawMagnetZ - (iMagMaxZ + iMagMinZ)) / (iMagMaxZ - iMagMinZ);
            g[0] = (float)(2 * moveController.inputReport.RawGyroYaw - (iGyrMaxX + iGyrMinX)) / (iGyrMaxX - iGyrMinX);
            g[1] = (float)(2 * moveController.inputReport.RawGyroPitch - (iGyrMaxY + iGyrMinY)) / (iGyrMaxY - iGyrMinY);
            g[2] = (float)(2 * moveController.inputReport.RawGyroRoll - (iGyrMaxZ + iGyrMinZ)) / (iGyrMaxZ - iGyrMinZ);

            float dt = 1.0f / 175.0f;

            //if (!((bool)dev.initialized)) 
            //    dev.seqnum = seqnum;
            //else 
            //    dt *= (seqnum - dev.seqnum) & 31;
            
            //dev.seqnum = seqnum;
            //update_tracker_AGM(ref dev, dt, a, g, m);

            Matrix mat = Matrix.Identity;
            float[][] R = new float[][]{new float[]{1,0,0}, new float[]{0,1,0}, new float[]{0,0,1}};

            // get Z axis from normalised accelerometer data (gravity)
            R[2][0] = a[0];
            R[2][1] = a[1];
            R[2][2] = a[2];
            normalize(R[2], out R[2]);

            // calculate EAST axis from Z axis and normalised magnetometer data
            R[0][0] = -m[0];
            R[0][1] = -m[1];
            R[0][2] = -m[2];
            cross(R[2], R[0], out R[0]);
            normalize(R[0], out R[0]);

            // calculate NORTH axis from Z axis and EAST axis
            cross(R[2], R[0], out R[1]);

            mat.M11 = R[0][0]; mat.M12 = R[0][1]; mat.M13 = R[0][2];
            mat.M21 = R[1][0]; mat.M22 = R[1][1]; mat.M23 = R[1][2];
            mat.M31 = R[2][0]; mat.M32 = R[2][1]; mat.M33 = R[2][2];
            //mat.M11 = dev.R[0][0]; mat.M12 = dev.R[0][1]; mat.M13 = dev.R[0][2];
            //mat.M21 = dev.R[1][0]; mat.M22 = dev.R[1][1]; mat.M23 = dev.R[1][2];
            //mat.M31 = dev.R[2][0]; mat.M32 = dev.R[2][1]; mat.M33 = dev.R[2][2];

            //mat = Matrix.Transpose(mat);

            //Vector3 vec;
            //float X = m_trans.Translation.X, Y = m_trans.Translation.Y, Z = m_trans.Translation.Z;
            //vec.X = dev.R[0][0]*Y + dev.R[0][1]*Y + dev.R[0][2]*Z;
            //vec.Y = dev.R[1][0]*Y + dev.R[1][1]*Y + dev.R[1][2]*Z;
            //vec.Z = dev.R[2][0]*Y + dev.R[2][1]*Y + dev.R[2][2]*Z;
            //m_trans.Translation = vec;
            //m_trans.Rotation = Quaternion.CreateFromRotationMatrix(mat);
            Vector3 p = new Vector3(dev.p[0], dev.p[1], dev.p[2]);

            Vector3 trans = Game.GraphicsDevice.Viewport.Unproject(p,
                                                            Core.ProjectionMatrix,
                                                            Core.ViewMatrix,
                                                            Matrix.Identity);

            if (bCalibrate)
            {
                m_matRef = mat;//Matrix.Transpose(mat);
                //return;
            }

            mat = m_matRef * mat;// Matrix.Multiply(m_matRef, mat);

#if OLD
            Vector3 t = m_trans.Translation;
            Matrix cam = Matrix.Identity;// CreateBillboard(m_trans.Translation, Vector3.Zero, Vector3.Up, Vector3.Forward);//Matrix.Identity; 
            cam.M11 = -1; cam.M22 = -1; cam.M33 = 1;
            mat = Matrix.Transpose(mat);
            //m_trans.WorldTransformation = Matrix.Multiply(Matrix.CreateFromQuaternion(Quaternion.CreateFromRotationMatrix(mat) * m_trans.Rotation), Matrix.CreateTranslation(t));
            mat = Matrix.Invert(mat);
            m_trans.Rotation = Quaternion.CreateFromRotationMatrix(mat);// *m_trans.Rotation;
#endif
            m_mat = Matrix.CreateFromQuaternion(box.Rotation);//Orientation);//m_trans.Rotation); 
            Matrix e = m_mat * mat; 
            mat = Matrix.Transpose(mat);//e);
            box.Rotation = Quaternion.CreateFromRotationMatrix(mat);//e);//m_trans.Rotation = Quaternion.CreateFromRotationMatrix(e);

        }

        /// <summary>
        /// Process the position and orientation with accelerometers, gyros and magnetometers.
        /// </summary>
#if OLD
        void update_tracker_AGM(ref tracker t, float dt, float[] A, float[] G, float[] M)
        {
            if (t.initialized == 0) 
            {
                t.A0 = A;
                absolute_orientation(A, M, out t.R);
                t.initialized = 1;
            }

            float[,] OmegaX = { {     0, -G[2],  G[1] },
		                        {  G[2],     0, -G[0] },
		                        { -G[1],  G[0],     0 } };

            float[][] oldR = oldR = t.R;//new float[3][];  
            //oldR = t.R;

            // Integrate rotation rate: R += R * Omega_X * dt
            for ( int i=0; i<3; ++i )
                for ( int j=0; j<3; ++j )
                    for ( int k=0; k<3; ++k )
                        t.R[i][j] += oldR[i][k] * OmegaX[k,j] * dt;

            // Bias R toward (noisy) absolute orientation: R = R0*k + R*(1-k)
            float kR = 0.01f;
            float[][] R0 = new float[3][];
            absolute_orientation(t.A0, M, out R0);
            
            for ( int i=0; i<3; ++i )
                for ( int j=0; j<3; ++j ) 
                    t.R[i][j] = R0[i][j] * kR + t.R[i][j] * (1 - kR);
            
            // Make R orthonormal
            normalize(t.R[2], out t.R[2]);
            cross(t.R[2], t.R[0], out t.R[1]);
            normalize(t.R[1], out t.R[1]);
            cross(t.R[1], t.R[2], out t.R[0]);

            // Rotate the lowpass-filtered acceleration: A0 -= (Omega^A0)*dt
            float[] dA0 = new float[3];  
            cross(G, t.A0, out dA0);
            
            for ( int i=0; i<3; ++i ) 
                t.A0[i] -= dA0[i] * dt;
  
            // Lowpass-filter the acceleration: A0 = A*k + A0*(1-k)
            float kA = 0.001f;
            for ( int i=0; i<3; ++i ) 
                t.A0[i] = A[i] * kA + t.A0[i] * (1 - kA);

            // Integrate acceleration: v += R * (A-A0)*dt
            float[] a = new float[3];
            for ( int i=0; i<3; ++i ) 
            {
                a[i] = 0;
                
                for ( int k=0; k<3; ++k ) 
                    a[i] += t.R[i][k] * (A[k] - t.A0[k]);
            }

            for ( int i=0; i<3; ++i ) 
                t.v[i] += a[i] * dt;

            // Integrate velocity: p += v*dt
            for ( int i=0; i<3; ++i ) 
                t.p[i] += t.v[i]*dt;

            // Damp to prevent integration drift on position and velocity.
            for ( int i=0; i<3; ++i ) 
            { 
                t.v[i]*=0.90f; 
                t.p[i]*=0.990f; 
            }
        }
#endif 
        
        void update_tracker_AGM(ref tracker t, float dt, float[] A, float[] G, float[] M)
        {
            if (t.initialized == 0)
            {
                t.A0 = A;
                absolute_orientation(A, M, out t.R);
                t.initialized = 1;
            }
            
            float[,] OmegaX = { {     0, -G[2],  G[1] },
		                        {  G[2],     0, -G[0] },
		                        { -G[1],  G[0],     0 } };

            float[][] oldR = t.R;

            // Integrate rotation rate: R += R * Omega_X * dt
            for (int i = 0; i < 3; ++i)
                for (int j = 0; j < 3; ++j)
                    for (int k = 0; k < 3; ++k)
                        t.R[i][j] += oldR[i][k] * OmegaX[k, j] * dt;

            // Bias R toward (noisy) absolute orientation: R = R0*k + R*(1-k)
            float kR = 0.01f;
            float[][] R0 = new float[3][];
            absolute_orientation(t.A0, M, out R0);

            for (int i = 0; i < 3; ++i)
                for (int j = 0; j < 3; ++j)
                    t.R[i][j] = R0[i][j] * kR + t.R[i][j] * (1 - kR);

            // Make R orthonormal
            normalize(t.R[2], out t.R[2]);
            cross(t.R[2], t.R[0], out t.R[1]);
            normalize(t.R[1], out t.R[1]);
            cross(t.R[1], t.R[2], out t.R[0]);

            // Rotate the lowpass-filtered acceleration: A0 -= (Omega^A0)*dt
            float[] dA0 = new float[3];
            cross(G, t.A0, out dA0);

            for (int i = 0; i < 3; ++i)
                t.A0[i] -= dA0[i] * dt;

            // Lowpass-filter the acceleration: A0 = A*k + A0*(1-k)
            float kA = 0.001f;
            for (int i = 0; i < 3; ++i)
                t.A0[i] = A[i] * kA + t.A0[i] * (1 - kA);

            // Integrate acceleration: v += R * (A-A0)*dt
            float[] a = new float[3];
            for (int i = 0; i < 3; ++i)
            {
                a[i] = 0;

                for (int k = 0; k < 3; ++k)
                    a[i] += t.R[i][k] * (A[k] - t.A0[k]);
            }

            for (int i = 0; i < 3; ++i)
                t.v[i] += a[i] * dt;

            // Integrate velocity: p += v*dt
            for (int i = 0; i < 3; ++i)
                t.p[i] += t.v[i] * dt;

            // Damp to prevent integration drift on position and velocity.
            for (int i = 0; i < 3; ++i)
            {
                t.v[i] *= 0.90f;
                t.p[i] *= 0.990f;
            }
        }

        /// <summary>
        /// Normalize a vector.
        /// </summary>
        void normalize(float[] u, out float[] res) 
        {
            float[] temp = new float[3];
            float k = 1.0f / (float)Math.Sqrt((double)(u[0]*u[0]+u[1]*u[1]+u[2]*u[2]));
            
            for ( int i=0; i<3; ++i ) 
                temp[i] = u[i] * k;

            res = temp;
        }

        /// <summary>
        /// Calculate the cross product
        /// </summary>
        void cross(float[] u, float[] v, out float[] res) 
        {
            float[] temp = new float[3];

            temp[0] = u[1]*v[2] - u[2]*v[1];
            temp[1] = u[2]*v[0] - u[0]*v[2];
            temp[2] = u[0]*v[1] - u[1]*v[0];

            res = temp;
        }

        /// <summary>
        /// Compute absolute orientation from accelerometers and magnetometers.
        /// Assume accelerometers point UP (-gravity only, device not accelerating).
        /// Assume magnetic field points SOUTH (with unknown elevation).
        /// /// </summary>
        /// <param name="A"></param>       
        /// <param name="M"></param>       
        /// <param name="R"></param>   
#if OLD
        void absolute_orientation(float[] A, float[] M, out float[][] R) 
        {
            float[][] temp = new float[3][];

            // A = -gravity = UP direction expressed in local coordinates => R[2].
            normalize(A, out temp[2]);

            // UP ^ MagneticField = EAST direction in local coordinates => R[0].
            cross(temp[2], M, out temp[0]);
            normalize(temp[0], out temp[0]);

            // UP ^ EAST = global NORTH direction in local coordinates => R[1].
            cross(temp[2], temp[0], out temp[1]);

            R = temp;
        }
#endif
        void absolute_orientation(float[] A, float[] M, out float[][] R)
        {
            float[][] temp = new float[3][];

            // A = -gravity = UP direction expressed in local coordinates => R[2].
            normalize(A, out temp[2]);

            // UP ^ MagneticField = EAST direction in local coordinates => R[0].
            //M[0] = -M[0]; M[1] = -M[1]; M[2] = -M[2];
            cross(temp[2], M, out temp[0]);
            normalize(temp[0], out temp[0]);

            // UP ^ EAST = global NORTH direction in local coordinates => R[1].
            cross(temp[2], temp[0], out temp[1]);

            R = temp;
        }

        //private void callbackDataProc(object obj)
        //{
        //    while (m_bCalibrated)
        //    {
        //        UpdateMoveTranslation();
        //        UpdateMoveRotation(false);
        //    }
        //}

        private void CreateModel()
        {
            // Enable the scene
            //if (!m_scene.Enabled)
            //    m_scene.Enabled = true;
#if OLD
            // Load the model
            ModelLoader loader = new ModelLoader();
            GoblinXNA.Graphics.Model model = (GoblinXNA.Graphics.Model)loader.Load("", "Hand");
            model.UseInternalMaterials = true;
            model.ReceiveShadows = true;

            // Create a geometry node
            GeometryNode node = new GeometryNode();
            node.Model = model;

            // Add the geometry node to a transformation node
            m_trans.AddChild(node);
            
            // Add the model to the scene via the geometry and transformation node
            m_scene.RootNode.AddChild(m_trans);
#endif
            //BoxObject box = new BoxObject(Vector3.Zero/*m_position*/, Vector3.One, 1f);
            box = new CylinderObject(Vector3.Zero/*m_position*/, 0.5f, 2f, 10, 1f);
            
            //m_trans.AddChild(box);
             //m_scene.RootNode.AddChild(box);//m_trans);


            // Initialize and start the image processing, which will be processed in
            // an own thread for performance gain.
            //timerDataProc = new Timer(new TimerCallback(callbackDataProc), null, 0, 20);
            //threadDataProc.Start();

            // Finally declare the calibration to be finished
            m_bCalibrated = true;
            imgProc.threading = true;
        }

        /// <summary>
        /// Create a Texture2D from the Playstation eye camera.
        /// </summary>
        public Texture2D GetCameraImage()
        {
            Texture2D tex = new Texture2D(graphicsDevice, 640, 480, false, SurfaceFormat.Color);

            int[] array = new int[640 * 480];

            int index = 0;

            // Determine the image stride
            int srcStride = 640 * 4;

            // Copy the data from the pointer of the PS Eye interface into the integer array
            // using only the RGB values
            //
            // Value:       R   |    G    |     B    |  unused
            // Bits:    0......7|8......15|16......23|24......31
            unsafe
            {
                byte* src = (byte*)(imgProc.Eye.ptrBmpPixels);

                for (int i = 0; i < 480; i++)
                {
                    for (int j = 0; j < srcStride; j += 4)
                    {
                        if (currentState == CalibrationState.NotCalibratable)
                        {
                            if (imgProc.result[new System.Drawing.Point(j >> 2, i)].Intensity > 0)
                                array[index++] = 255;
                            else
                                array[index++] = (int)((*(src + j) << 16) | (*(src + j + 1) << 8) | *(src + j + 2));
                        }
                        else
                            array[index++] = (int)((*(src + j) << 16) | (*(src + j + 1) << 8) | *(src + j + 2));
                    }

                    src += srcStride;
                }
            }

            // Create the texture from the camera image
            tex.SetData<int>(array, 0, 640 * 480);

            return tex;
        }

        public void Dispose()
        {
            imgProc.Dispose();
        }
    }
}


